/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.*;

/**
 *
 * @author jdmarler
 */


public class Arm {


    //public static final double PI = 3.14159265358979323846264338327950288;
    
    //public static final double ENCODER_DISTANCE_PER_PULSE = ((8.0/12)*PI)/(250*3); //.0027925268
    
    
    Talon  armMotor;
    Solenoid openSolenoid;
    Solenoid closeSolenoid;
    
    //Encoder LeftEncoder = new Encoder(HardwareMap.LEFT_DRIVE_ENCODER_CH_A,
            //HardwareMap.LEFT_DRIVE_ENCODER_CH_B, false, CounterBase.EncodingType.k2X);

    //Encoder RightEncoder = new Encoder(HardwareMap.RIGHT_DRIVE_ENCODER_CH_A,
            //HardwareMap.RIGHT_DRIVE_ENCODER_CH_B, true, CounterBase.EncodingType.k2X);

    public Arm(int armMotorPort, int openSolenoidPort, int closeSolenoidPort) {
         
       try {
           armMotor = new Talon(armMotorPort);
           armMotor.setSafetyEnabled(false);
           openSolenoid = new Solenoid(openSolenoidPort);
           closeSolenoid = new Solenoid(closeSolenoidPort);
           
           //Sets distance in inches
           //This was obtained by finding the circumfrance of the wheels,
           //finding the number of encoder pulses per rotation,
           //and dividing the circumfrance by that number.

           //LeftEncoder.setDistancePerPulse(ENCODER_DISTANCE_PER_PULSE);
           //RightEncoder.setDistancePerPulse(ENCODER_DISTANCE_PER_PULSE);
       }

       catch (Exception e){
           e.printStackTrace();
        }  
    }
  
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    public void setSpeed(double s){
        armMotor.set(s);
    }
    
    public void deploy(){
        openSolenoid.set(true);
        closeSolenoid.set(false);
    }
        
    public void retract(){
        openSolenoid.set(false);
        closeSolenoid.set(true);
        
    }
    
    //public double GetLeftEncoderValue(){
        //return(LeftEncoder.getDistance());
    //}
    
    //public double GetRightEncoderValue(){
       //return(RightEncoder.getDistance());
    //}
    //public void StartEncoders(){
        //LeftEncoder.start();
        //RightEncoder.start();
    //}

    //public void StopEncoders(){
        //LeftEncoder.stop();
        //RightEncoder.stop();
    //}

    //public void ResetEncoders(){
        //LeftEncoder.reset();
        //RightEncoder.reset();
    //}
}
